package ext.sim;

import context.render.vertexdata.VertexData;
import ext.sim.contact.Contact;
import ext.sim.contact.detect.ContactDetector;
import ext.sim.contact.ContactSolver;
import ext.sim.forces.Force;
import java.util.ArrayList;
import java.util.LinkedList;
import math.mat3;
import static math.math.*;
import math.vec3;
import math.vec4;

public class BodySystem
{
    
    public ArrayList<Body> mBodies;
    
    public ArrayList<Force> mForces;
    
    public ContactDetector mContactDetect;
    
    public ContactSolver mContactProc;
    
    
    public BodySystem()
    {
        this.mBodies = new ArrayList<>();
        this.mForces = new ArrayList<>();
        this.mContactDetect = new ContactDetector();
        this.mContactProc = new ContactSolver();
    }
    
    
    public void advance(double aDT)
    {
        for (Body b : this.mBodies)
        {
            b.mF.set(vec3.ZERO);
            b.mT.set(vec3.ZERO);
        }
        for (Force f : this.mForces)
        {
            f.apply();
        }
        
        for (Body b : this.mBodies)
        {
            b.transformInternalsToB();
            
            // INTEGRATE F = d/dt[P], and derive linear velocity.
            if (!b.mIsPinned)
            {
                b.mP.addmul(aDT, b.mF);
                
                // UPDATE V = P / Mass
                b.mV.set(b.mP);
                double rMass = 1.0 / b.mMass;
                b.mV.mul(rMass);
            }
            // INTEGRATE T = d/dt[L], and derive angular velocity.
            if (!b.mIsPinned)
            {
                b.mL.addmul(aDT, b.mT);
                
                // UPDATE R = MatrixFromQuaternion(Norm(Q))
                b.mR.set(this.matrixFromQuaternion(b.mQ));
                b.mInvR.set(b.mR);
                b.mInvR.transpose(); // Inverse(R) = Transpose(R)
                
                // UPDATE I = R * Ibody * Inverse(R)
                b.mI.set(b.mR);
                b.mI.mul(b.mIbody);
                b.mI.mul(b.mInvR);
                
                // UPDATE InvI = R * Inverse(Ibody) * Inverse(R)
                b.mInvI.set(b.mR);
                b.mInvI.mul(b.mInvIbody);
                b.mInvI.mul(b.mInvR);
                
                // UPDATE W: W = Inverse(I) * L
                b.mW.set(b.mInvI.mul(b.mL));
            }
            
            b.transformInternalsToW();
        }
        
        LinkedList<Contact> contacts = this.mContactDetect.detect(this.mBodies);
        
        this.mContactProc.solve(this.mBodies, contacts, aDT);
        
        for (Body b : this.mBodies)
        {
            b.transformInternalsToB(); // RETURN COLLISION STRUCTURES TO BODY-SPACE.
            
            // INTEGRATE V = d/dt[X]
            if (!b.mIsPinned)
            {
                b.mX.addmul(aDT, b.mV);
            }
            // INTEGRATE: d/dt[Q] = 1/2 * (W, 0) * Q, where (W, 0) is the quaternion with real scalar 0, imaginary vector W.
            if (!b.mIsPinned)
            {
                vec4 deltaQ = this.quatMul(new vec4(b.mW, 0.0), b.mQ);
                deltaQ.mul(0.5 * aDT);
                b.mQ.add(deltaQ);
                b.mQ.norm();
            }
            
            if (b instanceof BodyCuboid)
            {
                ((BodyCuboid) b).updateVertsAndNorms();
            }
            
            b.transformInternalsToW();
        }
        
        
    }
    
    
    public void render(VertexData aVertexData)
    {
        for (Body b : this.mBodies)
        {
            b.render(aVertexData);
        }
    }
    
    public void renderDebug(VertexData aVD)
    {
        for (Body b : this.mBodies)
        {
            b.renderDebug(aVD);
        }
    }
    
    
    private vec4 quatMul(vec4 a, vec4 b)
    {
        return new vec4(
            a.w * b.x + a.x * b.w - a.y * b.z + a.z * b.y, 
            a.w * b.y + a.x * b.z + a.y * b.w - a.z * b.x, 
            a.w * b.z - a.x * b.y + a.y * b.x + a.z * b.w, 
            a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z
        );
    }
    
    private mat3 matrixFromQuaternion(vec4 q)
    {
        mat3 m = new mat3();
        m.m[0] = 1.0 - 2.0 * (q.y * q.y + q.z * q.z);
        m.m[1] = 2.0 * (q.x * q.y + q.w * q.z);
        m.m[2] = 2.0 * (q.x * q.z - q.w * q.y);
        m.m[3] = 2.0 * (q.x * q.y - q.w * q.z);
        m.m[4] = 1.0 - 2.0 * (q.x * q.x + q.z * q.z);
        m.m[5] = 2.0 * (q.y * q.z + q.w * q.x);
        m.m[6] = 2.0 * (q.x * q.z + q.w * q.y);
        m.m[7] = 2.0 * (q.y * q.z - q.w * q.x);
        m.m[8] = 1.0 - 2.0 * (q.x * q.x + q.y * q.y);
        return m;
    }
    
    
    private vec4 quaternionFromMatrix(mat3 m)
    {
        double trace = m.m[0] + m.m[4] + m.m[8];
        double s;
        if (trace >= 0.0)
        {
            s = sqrt(trace + 1.0);
            double realComp = 0.5 * s;
            s = 0.5 / s;
            return new vec4(
                s * (m.m(2, 1) - m.m(1, 2)), 
                s * (m.m(0, 2) - m.m(2, 0)), 
                s * (m.m(1, 0) - m.m(0, 1)), 
                realComp
            );
        }
        int i = 0;
        if (m.m(1, 1) > m.m(0, 0))
        {
            i = 1;
        }
        if (m.m(2, 2) > m.m(i, i))
        {
            i = 2;
        }
        switch (i)
        {
            case 0:
                s = sqrt(m.m(0, 0) - m.m(1, 1) + m.m(2, 2) + 1.0);
                double iComp = 0.5 * s;
                s = 0.5 / s;
                return new vec4(
                    iComp, 
                    s * (m.m(0, 1) + m.m(1, 0)), 
                    s * (m.m(2, 0) + m.m(0, 2)), 
                    s * (m.m(2, 1) - m.m(1, 2))
                );
            case 1:
                s = sqrt(m.m(1, 1) - m.m(2, 2) + m.m(0, 0) + 1.0);
                double jComp = 0.5 * s;
                s = 0.5 / s;
                return new vec4(
                    s * (m.m(0, 1) + m.m(1, 0)), 
                    jComp, 
                    s * (m.m(1, 2) + m.m(2, 1)), 
                    s * (m.m(0, 2) - m.m(2, 0))
                );
            default: // case 2:
                s = sqrt(m.m(2, 2) - m.m(0, 0) + m.m(1, 1) + 1.0);
                double kComp = 0.5 * s;
                s = 0.5 / s;
                return new vec4(
                    s * (m.m(2, 0) + m.m(0, 2)), 
                    s * (m.m(1, 2) + m.m(2, 1)), 
                    kComp, 
                    s * (m.m(1, 0) - m.m(0, 1))
                );
        }
    }
    
    private mat3 crossMatrix(vec3 a)
    {
        mat3 m = new mat3();
        m.m[0] = m.m[4] = m.m[8] = 0.0;
        
        m.m[1] = a.z;
        m.m[3] = -a.z;
        
        m.m[2] = -a.y;
        m.m[6] = a.y;
        
        m.m[5] = a.x;
        m.m[7] = -a.x;
        return m;
    }
    
}
